// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ArmCubicSplineExecutionAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation.Runtime.Arm
{
    using System;
    using System.Collections.Generic;
    using System.Collections.ObjectModel;
    using System.Diagnostics;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Manipulation.Runtime.MotionControl;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;

    /// <summary>
    /// Agent that executes a cubic spline in arm joint space
    /// </summary>
    [DataContract]
    public class ArmCubicSplineExecutionAgent :
                            ConsumerAgent<ManipulatorStatusMessage>,
                            IControlReceiver<StartTrajectoryMessage>
    {
        /// <summary>
        /// Initializes a new instance of the <see cref="ArmCubicSplineExecutionAgent" /> class
        /// </summary>
        /// <param name="name">Agent name.</param>
        /// <param name="producer">TimerMessage Producer</param>
        /// <param name="armJointControlAgentName">Name of agent accepting joint control commands.</param>
        /// <param name="jointErrorLimit">Maximum allowed difference in radians between commanded an actual position.</param>
        public ArmCubicSplineExecutionAgent(
            string name,
            IProducer<ManipulatorStatusMessage> producer,
            string armJointControlAgentName,
            double jointErrorLimit)
            : base(name, producer.Name)
        {
            this.ArmJointControlAgentName = armJointControlAgentName;
            this.JointErrorLimit = jointErrorLimit;
        }

        /// <summary>
        /// Gets or sets a value indicating whether or not we are currently processing a spline.
        /// </summary>
        protected bool InSpline { get; set; }

        /// <summary>
        /// Gets or sets a value indicating whether the first point of the spline should be set to the current location/velocity
        /// before executing spline.
        /// </summary>
        protected bool NeedToUpdateFirstPointWithCurrentPosition { get; set; }

        /// <summary>
        /// Gets or sets a value indicating whether or not velocities should be updated to ensure C2 continuity
        /// </summary>
        protected bool NeedToUpdateVelocities { get; set; }

        /// <summary>
        /// Gets or sets a value indicating whether or not velocities of the first point at time > 0 should be 
        /// updated for C2 continuity or held at zero
        /// </summary>
        protected bool EnforceZeroVelocityAtBeginningOfSpline { get; set; }

        /// <summary>
        /// Gets or sets current spline
        /// </summary>
        protected CubicSpline CurrentSpline { get; set; }

        /// <summary>
        /// Gets or sets timer for current spline 
        /// </summary>
        protected Stopwatch SplineTimer { get; set; }

        /// <summary>
        /// Gets or sets forwarder for agent accepting joint control commands.
        /// </summary>
        protected IForwarder<JointGoalPositionControlMessage> ArmJointControlPort { get; set; }

        /// <summary>
        /// Gets or sets name of the agent to accept joint control commands.
        /// </summary>
        [DataMember(Name = "ArmJointControlAgentName")]
        protected string ArmJointControlAgentName { get; set; }

        /// <summary>
        /// Gets or sets maximum value in radians that the goal position for a joint can be from it's current position.
        /// </summary>
        [DataMember(Name = "JointErrorLimit")]
        protected double JointErrorLimit { get; set; }

        /// <summary>
        /// Called when agent is initialized.
        /// </summary>
        /// <param name="locator">Agent locator.</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            IAgentHost armControlAgent = locator.GetAgent(this.ArmJointControlAgentName);
            this.ArmJointControlPort = armControlAgent.GetControlPort<JointGoalPositionControlMessage>();
        }

        /// <summary>
        /// Receive StartCubicSplineMessage and initiate a new cubic spline motion
        /// </summary>
        /// <param name="message">A StartCubicSplineMessage message.</param>
        public void ReceiveControl(StartTrajectoryMessage message)
        {
            double[] timesCopy = new double[message.Times.Count];
            message.Times.CopyTo(timesCopy, 0);

            if (timesCopy[0] < 0)
            {
                throw new ArgumentException(string.Format("Expected value > 0 for first time, found {0}", timesCopy[0]));
            } 

            double lastTime = timesCopy[0];

            for (int i = 1; i < timesCopy.Length; ++i)
            {
                if (lastTime >= timesCopy[i])
                {
                    throw new ArgumentException("Spline times are not monotonically increasing");
                }

                lastTime = timesCopy[i];
            }

            this.CurrentSpline = new CubicSpline(
                message.NumDOF,
                message.NumPoints,
                timesCopy,
                message.TargetPositions.GetCopyAsArray(),
                message.TargetVelocities.GetCopyAsArray());
                
            this.InSpline = true;
            this.NeedToUpdateFirstPointWithCurrentPosition = message.UseCurrentPositionAsFirstPoint;
            this.NeedToUpdateVelocities = message.AdjustVelocitiesForContinuity;
            this.EnforceZeroVelocityAtBeginningOfSpline = message.EnforceZeroVelocityAtStartOfTrajectory;
            this.SplineTimer = Stopwatch.StartNew();
        }

        /// <summary>
        /// Called when a ManipulatorStatusMessage is received.
        /// If in a spline, gets the current set point of the spline and publishes a joint control message.
        /// </summary>
        /// <param name="message">A ManipulatorStatusMessage message.</param>
        public override void Receive(ManipulatorStatusMessage message)
        {
            if (!this.InSpline)
            {
                return;
            }

            if (message.State.JointPositions.Length != this.CurrentSpline.NumDOF)
            {
                throw new ApplicationException("Mismatch between num DOF of spline and manipulator");
            } 

            this.UpdateFirstPointIfRequired(message);
            this.UpdateVelocitiesIfRequired();

            double[] positions = this.CurrentSpline.GetPositions(this.SplineTimer.Elapsed.TotalSeconds);

            this.ExecutePositions(message, positions);
        }
                
        /// <summary>
        /// Sends spline commands if positions are valid, otherwise terminates execution of this spline
        /// </summary>
        /// <param name="message">A ManipulatorStatusMessage.</param>
        /// <param name="positions">Array of positions</param>
        public void ExecutePositions(ManipulatorStatusMessage message, double[] positions)
        {
            bool endOfSpline = false;

            // if we are off the end of the spline
            // get final position of spline as joint position reference
            if (positions == null)
            {
                positions = this.CurrentSpline.GetPositions(this.CurrentSpline.Times[this.CurrentSpline.NumPoints - 1]);
                endOfSpline = true;
            }

            // if JointErrorLimit <= 0, then no limits are imposed
            if (this.JointErrorLimit > 0)
            {
                double error;
                for (int i = 0; i < positions.Length; ++i)
                {
                    error = positions[i] - message.State.JointPositions[i];
                    if (Math.Abs(error) > this.JointErrorLimit)
                    {
                        positions[i] = message.State.JointPositions[i] + Math.Sign(error) * this.JointErrorLimit;

                        // if we are adjusting positions still, we are not sending final position in spline
                        // so keep going
                        endOfSpline = false;
                    }
                }
            }

            JointGoalPositionControlMessage jointControlMessage = new JointGoalPositionControlMessage(
                message.OriginatingTime,
                positions,
                0);

            this.ArmJointControlPort.Post(jointControlMessage);

            if (endOfSpline)
            {
                this.InSpline = false;
            }
        }

        /// <summary>
        /// Handles updating beginning point of spline if required.
        /// </summary>
        /// <param name="message">A ManipulatorStatusMessage.</param>
        protected void UpdateFirstPointIfRequired(ManipulatorStatusMessage message)
        {
            if (this.NeedToUpdateFirstPointWithCurrentPosition)
            {
                for (int i = 0; i < this.CurrentSpline.NumDOF; ++i)
                {
                    this.CurrentSpline.TargetPositions[i, 0] = message.State.CommandedJointPositions[i];

                    // for now assume that arm is at rest when spline starts
                    // may need to revisit assumption later
                    this.CurrentSpline.TargetVelocities[i, 0] = 0;
                }

                this.NeedToUpdateFirstPointWithCurrentPosition = false;
            }
        }

        /// <summary>
        /// Updates velocities for C2 continuity of not already done
        /// </summary>
        protected void UpdateVelocitiesIfRequired()
        {
            if (this.NeedToUpdateVelocities)
            {
                this.CurrentSpline.FixIntermediatePointVelocities(this.EnforceZeroVelocityAtBeginningOfSpline);
                this.NeedToUpdateVelocities = false;
            }
        }
    }
}
